#include <rclcpp/rclcpp.hpp>
#include <geometry_msgs/msg/twist.hpp>
#include <geometry_msgs/msg/vector3.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <std_msgs/msg/string.hpp>
#include <std_msgs/msg/int32_multi_array.hpp>
#include <std_msgs/msg/float32_multi_array.hpp>
#include <Eigen/Dense>
#include <vector>
#include <string>
#include <cmath>
#include <algorithm>
#include <geometry_msgs/msg/vector3.hpp>
#include <map>
#include <uav_swarms/KM.hpp>
#include <uav_swarms/Generator_queue.hpp>
#include "mavros_msgs/msg/state.hpp"
#include <mavros_msgs/srv/set_mode.hpp>
#include <mavros_msgs/srv/command_bool.hpp>
#include <geometry_msgs/msg/twist_stamped.hpp>
#include <yolo_detect_message/msg/call_server_status.hpp>
#include <yolo_detect_message/srv/service.hpp>
class Leader : public rclcpp::Node {
public:
    Leader();
    Eigen::MatrixXd get_communication_topology(const Eigen::MatrixXd &rel_posi);
    bool find_path(int i);
    std::vector<int> KM();
    std::vector<std::string> formation_dicts {};
    std::map<std::string,std::vector<Eigen::Vector3d>> formation_dict;

    void service_callback(const yolo_detect_message::srv::Service::Request::SharedPtr req,
                        const yolo_detect_message::srv::Service::Response::SharedPtr res);

    void initialize_parameters() {
        // 声明参数，确保名称和类型一致
        this->declare_parameter<int>("vehicle_id", 0);
        this->declare_parameter<std::string>("uav_namespace", "uav_swarms");
        this->declare_parameter<std::string>("vehicle_type", "drone");
        this->declare_parameter<int>("vehicle_num", 3);

        // 获取参数值
        uav_type = this->get_parameter("vehicle_type").as_string();
        namespaces = this->get_parameter("uav_namespace").as_string();
        uav_id = this->get_parameter("vehicle_id").as_int();  // 获取整数类型的 vehicle_id
        uav_num = this->get_parameter("vehicle_num").as_int();  // 获取整数类型的 vehicle_num

        // 输出日志
        RCLCPP_INFO(this->get_logger(), "Namespace: %s", namespaces.c_str());
        RCLCPP_INFO(this->get_logger(), "Vehicle Type: %s", uav_type.c_str());
        RCLCPP_INFO(this->get_logger(), "Vehicle ID: %d", uav_id);  // 使用 %d 占位符
        RCLCPP_INFO(this->get_logger(), "Vehicle Num: %d", uav_num);  // 使用 %d 占位符
    }

private:

    // 设置无人机ID
    int uav_id;

    // 无人机的位置信息
    geometry_msgs::msg::PoseStamped pose;

    // 无人机的速度
    geometry_msgs::msg::Twist cmd_vel_enu;

    // 设置无人机的数量
    int uav_num;

    // 避障速度
    geometry_msgs::msg::Vector3 avoid_vel;

    // 编队配置的等待信息
    std::string formation_config = "waiting";

    // 初始编队配置位置相关信息
    std::vector<Eigen::Vector3d> origin_formation;

    // 新编对配置位置相关信息
    std::vector<Eigen::Vector3d> new_formation;

    // 邻接矩阵
    std::vector<double> label_left_;
    std::vector<double> label_right_;

    Eigen::MatrixXi adj_matrix_;
    std::vector<int> match_right;

    std::vector<bool> visit_left_;
    std::vector<bool> visit_right_;
    
    // 通信拓扑
    std::vector<std::vector<int>> communication_topology_;
    
    // 已更改ID的数组
    std::vector<int> changed_id;

    // 目标高度记录标志
    bool target_height_recorded = false;

    // 字符串命令
    std_msgs::msg::String cmd;

    double f = 200.0;

    double Kz = 0.5;

    // ROS2 Publishers and Subscribers
    rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr pose_sub_;
    rclcpp::Subscription<geometry_msgs::msg::Pose>::SharedPtr target_pose_sub_;
    rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_sub_;
    rclcpp::Subscription<geometry_msgs::msg::Vector3>::SharedPtr avoid_vel_sub_;
    rclcpp::Subscription<std_msgs::msg::String>::SharedPtr leader_cmd_sub_;
    rclcpp::Subscription<geometry_msgs::msg::TwistStamped>::SharedPtr leader_velocity_sub_;

    rclcpp::Subscription<yolo_detect_message::msg::CallServerStatus>::SharedPtr server_status_sub_;

    rclcpp::Publisher<geometry_msgs::msg::Pose>::SharedPtr pose_enu_pub_;
    rclcpp::Publisher<std_msgs::msg::Float32MultiArray>::SharedPtr formation_pattern_pub_;
    rclcpp::Publisher<std_msgs::msg::Int32MultiArray>::SharedPtr communication_topology_pub_;
    rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr vel_flu_pub_;
    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr cmd_pub_;

    rclcpp::Publisher<geometry_msgs::msg::TwistStamped>::SharedPtr leader_velocity_pub_;

    rclcpp::Subscription<mavros_msgs::msg::State>::SharedPtr state_sub_;

    rclcpp::Client<mavros_msgs::srv::SetMode>::SharedPtr set_mode_client_;
    rclcpp::Client<mavros_msgs::srv::CommandBool>::SharedPtr arming_client_;

    rclcpp::Service<yolo_detect_message::srv::Service>::SharedPtr service_;
    geometry_msgs::msg::PoseStamped pose_;

    geometry_msgs::msg::Vector3 avoid_velocity_;

    geometry_msgs::msg::Twist cmd_vel_enu_;

   
    std::vector<double> slack_right_;
    rclcpp::TimerBase::SharedPtr timer_;

    std::mutex leader_velocity_mutex_;
    geometry_msgs::msg::TwistStamped leader_velocity_;
    // 回调函数，用于更新无人机的位姿
    void pose_callback(const geometry_msgs::msg::PoseStamped::SharedPtr msg); 

    void leader_velocity_callback(const geometry_msgs::msg::TwistStamped::SharedPtr msg);

    // 回调函数，用于发布无人机的位姿
    void target_pose_callback(const geometry_msgs::msg::Pose::SharedPtr msg);
    // 回调函数，用于处理避障速度
    void avoid_vel_callback(const geometry_msgs::msg::Vector3::SharedPtr msg);

    // 速度指令回调函数
    void cmd_vel_callback(const geometry_msgs::msg::Twist::SharedPtr msg);

    void server_status_callback(const yolo_detect_message::msg::CallServerStatus::SharedPtr msg);

    void cmd_callback(const std_msgs::msg::String::SharedPtr msg); 
    double calculation_norm(Eigen::Vector3d a, Eigen::Vector3d b);

    // 处理指令（改变编队配置）
    void state_callback(const mavros_msgs::msg::State::SharedPtr msg);
    Eigen::MatrixXi build_graph(std::vector<Eigen::Vector3d> origin_formation, std::vector<Eigen::Vector3d> change_formation);
    

    std::vector<Eigen::Vector3d> get_new_formation(const std::vector<int>& changed_id, const std::vector<Eigen::Vector3d>& change_formation);
     // 控制循环


    void control_loop();

    // 通信拓扑生成
    std::vector<std::vector<int>> get_communication_topology(const std::vector<Eigen::Vector3d>& rel_posi);


    std::shared_ptr<Generator_queue> generator_queue;


    std::string namespaces;

    std::string uav_type;
    geometry_msgs::msg::Pose target_pose_;
    int control_flag = 0;

    mavros_msgs::msg::State current_state_;
    rclcpp::Time last_request_;
    bool initial_setup_  = false;

    std::mutex server_status_mutex_;

    std::atomic<bool> service_status{false};
};
